﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;

using USB;
using System.Diagnostics;
using System.Threading;

namespace SignalGen
{
    public class Hardware
    {
        private bool TESTMODE = false;

        public delegate void ConnectDelegate(Device dev);
        public delegate void DisconnectDelegate();

        public volatile bool Connected;

        private Dictionary<TaskType, Task> Tasks = new System.Collections.Generic.Dictionary<TaskType, Task>();
        private Thread USBCommThread;
        private USBWrapper uw;
        private Timer tmrConnect;
        private ConnectDelegate cddc;
        private DisconnectDelegate cddd;
        private Device device;

        private double currentfreq = 0;
        private double currentphase = 0;

        public Hardware(ConnectDelegate connect, DisconnectDelegate disconnect)
        {
            cddc = connect;
            cddd = disconnect;

            Connected = false;
            tmrConnect = new Timer(new TimerCallback(tmrConnect_Tick));
        }

        public Packet Execute(Packet packet)
        {
            if (Connected == false) return null;

            Debug.WriteLine("Sent " + packet.Type + ": " + BitConverter.ToString(packet.GetData()).Replace('-', ' '));

            byte[] buf = packet.GetData();
            byte[] rcvbuf = new byte[64];
            Packet rcv = new Packet();

            short sent = uw.Write(buf, 100);
            if (sent != buf.Length)
            {
                Debug.WriteLine("Send Error");
                Disconnect();
                Connect();
                return null;
            }

            short rec = uw.Read(ref rcvbuf, 100);
            rcv.SetData(rcvbuf);
            if (rec != rcv.Data.Length + 2)
            {
                Debug.WriteLine("Receive Error");
                Disconnect();
                Connect();
                return null;
            }

            Debug.WriteLine("Received : " + BitConverter.ToString(rcv.GetData()).Replace('-', ' '));
            return rcv;
        }

        private void tmrConnect_Tick(object sender)
        {
            //tmrConnect.Enabled = false;
            //tmrConnect.Change(Timeout.Infinite, Timeout.Infinite);

            if (Connected == false)
            {
                Connect();
            }
            else
            {
                Debug.WriteLine("SignalGen Ping...");
                Packet r = Execute(new Packet(Packet.eType.GET_VERSION, new byte[0]));
            }
        }

        public void Connect()
        {
            short sRet;
            byte[] descriptor = new byte[256];
            byte[] str0 = new byte[256]; short str0len = 0;
            byte[] str1 = new byte[256]; short str1len = 0;
            byte[] str2 = new byte[256]; short str2len = 0;
            byte[] str3 = new byte[256]; short str3len = 0;

            if (Connected == true) return;

            if (TESTMODE == true)   // To test without an actual hw device
            {
                device = new Device();
                Connected = true;
                //tmrConnect.Change(Timeout.Infinite, Timeout.Infinite);
                USBCommThread = new Thread(new ThreadStart(USBCommThreadProc));
                USBCommThread.Start();
                if (cddc != null) cddc(device);
                return;
            }

            device = new Device();
            device.DriverVersion = null;
            device.Manufacturer = null;
            device.Product = null;
            device.Serial = null;

            uw = new USBWrapper();

            sRet = uw.Initialize();
            if (sRet == 0)
            {
                uint version = uw.GetDriverVersion();
                short devnum = uw.GetDeviceCount("vid_04d8&pid_000c");

                if (devnum > 0)
                {
                    for (int instance = 0; instance < devnum; instance++)
                    {
                        sRet = uw.Open(instance, "vid_04d8&pid_000c", "\\MCHP_EP1", "\\MCHP_EP1", false);
                        if (sRet == 0) break;
                    }

                    if (sRet == 0)
                    {
                        sRet = uw.GetDeviceDescriptor(ref descriptor);

                        if (sRet >= 0)
                        {                            
                            str0len = uw.GetStringDescriptor(0, 0, ref str0);
                            if (descriptor[14] != 0)
                                str1len = uw.GetStringDescriptor(descriptor[14], 0, ref str1);
                            if (descriptor[15] != 0)
                                str2len = uw.GetStringDescriptor(descriptor[15], 0, ref str2);
                            if (descriptor[16] != 0)
                                str3len = uw.GetStringDescriptor(descriptor[16], 0, ref str3);

                            if (str1len > 0)
                                device.Manufacturer = System.Text.UnicodeEncoding.Unicode.GetString(str1, 2, str1len - 2);
                            if (str2len > 0)
                                device.Product = System.Text.UnicodeEncoding.Unicode.GetString(str2, 2, str2len - 2);
                            if (str3len > 0)
                                device.Serial = System.Text.UnicodeEncoding.Unicode.GetString(str3, 2, str3len - 2);

                            device.DriverVersion = (version / 0x1000000) + "." + ((version & 0x00FFFFFF) / 0x10000) + "." + ((version & 0x00FFFF) / 0x100) + "." + (version & 0x00FF);

                            Connected = true;
                            //tmrConnect.Enabled = false;
                            //tmrConnect.Change(Timeout.Infinite, Timeout.Infinite);

                            Packet r = Execute(new Packet(Packet.eType.GET_VERSION, new byte[0]));
                            device.FirmwareVersion = r.Data[1] + "." + r.Data[0];

                            r = Execute(new Packet(Packet.eType.READ_PROPS, new byte[0]));
                            device.FPGASampleNum = (r.Data[0] << 24) + (r.Data[1] << 16) + (r.Data[2] << 8) + r.Data[3];
                            device.FPGAFreq = ((r.Data[4] << 24) + (r.Data[5] << 16) + (r.Data[6] << 8) + r.Data[7]) * 1000;
                            device.FPGASampleResolution = r.Data[8];
                            device.FPGAPhaseResolution = r.Data[9];
                            device.FPGAAccResolution = r.Data[10];
                            device.FPGAFreqResolution = r.Data[11];

                            Tasks.Clear();
                            USBCommThread = new Thread(new ThreadStart(USBCommThreadProc));
                            USBCommThread.Start();

                            FpgaHardReset();
                            FpgaStartGen(currentfreq, currentphase);
                            // TODO: initialize dac

                            if (cddc != null) cddc(device);

                            return;
                        }
                    }
                }
            }

            //tmrConnect.Enabled = true;
            tmrConnect.Change(1000, 1000);
            //device = null;
            if (cddd != null) cddd();

            return;
        }

        public void Disconnect()
        {
            if (Connected == false) return;

            Connected = false;
            if (Thread.CurrentThread != USBCommThread)
                if (USBCommThread.Join(1000) == false)
                {
                    USBCommThread.Abort();
                }

            tmrConnect.Change(Timeout.Infinite, Timeout.Infinite);

            if (TESTMODE == false)
            {
                uw.Close();
            }

            if (cddd != null) cddd();
        }

        internal void AddTask(Task t)
        {
            // TODO: is this needed?
            //if (Connected == false) return;
            try
            {
                Monitor.Enter(Tasks);

                if (Tasks.ContainsKey(t.type) == true)
                    Tasks.Remove(t.type);

                Tasks.Add(t.type, t);
            }
            finally
            {
                Monitor.Exit(Tasks);
            }
        }

        private void USBCommThreadProc()
        {
            while (Connected == true)
            {
                Task ct = null;

                try
                {
                    Monitor.Enter(Tasks);
                    if (Tasks.Count > 0)
                    {
                        var en = Tasks.Values.GetEnumerator();
                        en.MoveNext();
                        ct = en.Current;
                        Tasks.Remove(ct.type);
                    }
                }
                finally
                {
                    Monitor.Exit(Tasks);
                }

                if (ct == null)
                {
                    Thread.Sleep(100); continue;
                }

                switch (ct.type)
                {
                    case TaskType.freq:
                        FreqTask ft = (FreqTask)ct;
                        SetFreq(ft);
                        break;
                    case TaskType.wave:
                        WaveTask wt = (WaveTask)ct;
                        SetWave(wt);
                        break;
                    case TaskType.phase:
                        PhaseTask pt = (PhaseTask)ct;
                        SetPhase(pt);
                        break;
                    case TaskType.offset:
                        OffsetTask ot = (OffsetTask)ct;
                        SetOffset(ot);
                        break;
                    case TaskType.amp:
                        AmpTask at = (AmpTask)ct;
                        SetAmp(at);
                        break;
                    case TaskType.sweep:
                        SweepTask st = (SweepTask)ct;
                        SetSweep(st);
                        break;
                    case TaskType.cursor:
                        break;
                }
            }
        }

        private void SetSweep(SweepTask st)
        {
            Debug.Write("SetSweep : start=" + st.startfreq + "; end=" + st.endfreq + "; cursor=" + st.cursorfreq + "; mode=" + st.mode);
            FpgaSetSweep(st.startfreq, st.endfreq, st.cursorfreq, st.mode);
        }

        private void SetAmp(AmpTask at)
        {
            Debug.Write("SetAmp : " + at.amp);
            FpgaSetDac(3, (short)at.amp);
        }

        private void SetOffset(OffsetTask ot)
        {
            Debug.Write("SetOffset : " + ot.offset);
            FpgaSetDac(2, (short)(device.MaxOffset - (short)ot.offset));    // offset is inverted
        }

        private void SetWave(WaveTask wt)
        {
            Debug.Write("SetWave : " + BitConverter.ToString(wt.wave));
            FpgaSetWave(wt.wave);
            FpgaStartGen(currentfreq, currentphase);
        }

        private void SetPhase(PhaseTask pt)
        {
            Debug.Write("SetPhase : " + pt.phase);
            currentphase = pt.phase;
            FpgaSetFreq(currentfreq, currentphase);
        }

        private void SetFreq(FreqTask ft)
        {
            Debug.Write("SetFreq : " + ft.freq);
            currentfreq = ft.freq;
            FpgaSetFreq(currentfreq, currentphase);
        }

        public void FpgaSetWave(byte[] wave)
        {
            if (TESTMODE == true)   // To test without an actual hw device
            {
                Debug.WriteLine(" - FpgaSetWave: " + BitConverter.ToString(wave));
                return;
            }

            // Init write
            Packet r = Execute(new Packet(Packet.eType.FPGAHARDRESET, new byte[] { 0xff, 0xff }));

            // Init write
            r = Execute(new Packet(Packet.eType.INITWRITE, new byte[] {0xff, 0xff}));

            // Write ram
            for (int i = 0; i < wave.Length; i += 62)
            {
                int writelen = (i < (wave.Length - 62) ? 62 : (wave.Length - i));
                byte[] data = new byte[writelen];
                Array.Copy(wave, i, data, 0, writelen);
                r = Execute(new Packet(Packet.eType.WRITERAM, data));
            }
        }

        public void FpgaStartGen(double freq, double phase)
        {
            if (TESTMODE == true)   // To test without an actual hw device
            {
                Debug.WriteLine(" - FpgaStartGen: freq = " + freq + "; phase = " + phase);
                return;
            }

            UInt32 ifreq = (UInt32)(freq / device.MinStep) + 1;
            UInt16 iphase = (UInt16)(phase);

            byte[] bfreq = BitConverter.GetBytes(ifreq); if (BitConverter.IsLittleEndian == false) bfreq = bfreq.Reverse().ToArray();
            byte[] fbfreq = new byte[4];
            bfreq.CopyTo(fbfreq, 0);

            byte[] bphase = BitConverter.GetBytes(iphase); if (BitConverter.IsLittleEndian == false) bphase = bphase.Reverse().ToArray();
            byte[] fbphase = new byte[2];
            bphase.CopyTo(fbphase, 0);

            byte[] data = new byte[7];
            Array.Copy(fbfreq, 0, data, 0, fbfreq.Length);
            Array.Copy(fbphase, 0, data, 4, fbphase.Length);
            data[6] = 0x00; // Internal CLock
            //data[6] = 0x01; // External CLock+

            Packet r = Execute(new Packet(Packet.eType.STARTGEN, data));
        }

        public void FpgaManualClk()
        {
            if (TESTMODE == true)   // To test without an actual hw device
            {
                Debug.WriteLine(" - FpgaManualClk ");
                return;
            }

            Packet r = Execute(new Packet(Packet.eType.MANCLK, new byte[1]));
        }

        public void FpgaHardReset()
        {
            if (TESTMODE == true)   // To test without an actual hw device
            {
                Debug.WriteLine(" - FpgaHardReset ");
                return;
            }

            Packet r = Execute(new Packet(Packet.eType.FPGAHARDRESET, new byte[1]));
        }

        public void FpgaSetDac(byte dac, short value)
        {
            if (TESTMODE == true)   // To test without an actual hw device
            {
                Debug.WriteLine(" - FpgaSetDac: dac = " + dac + "; value = " + value);
                return;
            }

            byte[] data = new byte[3];
            data[0] = dac;
            data[1] = (byte)(value / 256);
            data[2] = (byte)(value % 256);
            Packet r = Execute(new Packet(Packet.eType.SETDAC, data));
        }

        public void FpgaSetFreq(double freq, double phase)
        {
            if (TESTMODE == true)   // To test without an actual hw device
            {
                Debug.WriteLine(" - FpgaSetFreq: freq = " + freq + "; phase = " + phase);
                return;
            }

            UInt32 ifreq = (UInt32)(freq / device.MinStep) + 1;
            UInt16 iphase = (UInt16)(phase);

            byte[] bfreq = BitConverter.GetBytes(ifreq); if (BitConverter.IsLittleEndian == false) bfreq = bfreq.Reverse().ToArray();
            byte[] fbfreq = new byte[4];
            bfreq.CopyTo(fbfreq, 0);

            byte[] bphase = BitConverter.GetBytes(iphase); if (BitConverter.IsLittleEndian == false) bphase = bphase.Reverse().ToArray();
            byte[] fbphase = new byte[2];
            bphase.CopyTo(fbphase,0);

            byte[] data = new byte[6];
            Array.Copy(fbfreq, 0, data, 0, fbfreq.Length);
            Array.Copy(fbphase, 0, data, 4, fbphase.Length);

            Packet r = Execute(new Packet(Packet.eType.SETFREQ, data));
        }

        private void FpgaSetSweep(double sf, double ef, double cf, byte mode)
        {
            if (TESTMODE == true)   // To test without an actual hw device
            {
                Debug.WriteLine(" - FpgaSetSweep: sf= " + sf + "; ef = " + ef + "; cf= " + cf + "; mode=" + mode);
                return;
            }

            UInt32 isf = (UInt32)(sf / device.MinStep) + 1;
            UInt32 ief = (UInt32)(ef / device.MinStep) + 1;
            UInt32 icf = (UInt32)(cf / device.MinStep) + 1;

            byte[] bsfreq = BitConverter.GetBytes(isf); if (BitConverter.IsLittleEndian == false) bsfreq = bsfreq.Reverse().ToArray();
            byte[] fbsfreq = new byte[4];
            bsfreq.CopyTo(fbsfreq, 0);

            byte[] befreq = BitConverter.GetBytes(ief); if (BitConverter.IsLittleEndian == false) befreq = befreq.Reverse().ToArray();
            byte[] fbefreq = new byte[4];
            befreq.CopyTo(fbefreq, 0);

            byte[] bcfreq = BitConverter.GetBytes(icf); if (BitConverter.IsLittleEndian == false) bcfreq = bcfreq.Reverse().ToArray();
            byte[] fbcfreq = new byte[4];
            bcfreq.CopyTo(fbcfreq, 0);

            byte[] data = new byte[13];
            Array.Copy(fbsfreq, 0, data, 0, fbsfreq.Length);
            Array.Copy(fbefreq, 0, data, 4, fbefreq.Length);
            Array.Copy(fbcfreq, 0, data, 8, fbcfreq.Length);
            data[12] = mode;

            Packet r = Execute(new Packet(Packet.eType.SETSWEEP, data));
        }

    }

}
